#include "MMMLegacyMotionViewerWindow.h"

#include <QFileDialog>
#include <Eigen/Geometry>

#include <time.h>
#include <vector>
#include <iostream>

#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>

#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoBaseColor.h>
#include <Inventor/nodes/SoClipPlane.h>
#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoLightModel.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoScale.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoBaseColor.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoTranslation.h> 
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoLineSet.h>
#include <Inventor/nodes/SoUnits.h>
#include <Inventor/actions/SoGLRenderAction.h>
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/actions/SoLineHighlightRenderAction.h>
#include <Inventor/Qt/SoQt.h>
#include <Inventor/nodes/SoBaseColor.h>
#include <Inventor/nodes/SoPointSet.h>
#include <Inventor/nodes/SoComplexity.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/Trajectory.h>
#include <ctime>
#include <sstream>
#include <iostream>
#include <fstream>

#include <boost/algorithm/string.hpp>



#include <MMM/Motion/Legacy/LegacyMotion.h>
#include <MMM/Model/Model.h>

#include <MMM/Model/ModelReaderXML.h>
#include <MMMSimoxTools/MMMSimoxTools.h>


using namespace std;
using namespace VirtualRobot;

//#define TIMER_MS 200.0f // 5fps
#define TIMER_MS 10 //40.0f // 25fps

MMMLegacyMotionViewerWindow::MMMLegacyMotionViewerWindow(const std::string &dataFile, const std::string &markerMotionFile)
	:QMainWindow(NULL), _nLastPosition(0), _bShowValues(true)
{
	VR_INFO << " start " << endl;

	sceneSep = new SoSeparator;
	sceneSep->ref();
	SoUnits *u = new SoUnits();
	u->units = SoUnits::MILLIMETERS;
	sceneSep->addChild(u);

	motionSep = new SoSeparator;
	sceneSep->addChild(motionSep);
	rootCoordSep = new SoSeparator;
	sceneSep->addChild(rootCoordSep);
    markerSep = new SoSeparator;
    sceneSep->addChild(markerSep);

    floorSep = new SoSeparator;
    swFloor = new SoSwitch;

    // TODO: re-insert floor
    sceneSep->addChild(swFloor);
    swFloor->addChild(floorSep);

    // sceneSep->addChild(floorSep); ohne SoSwitch
    swFloor->whichChild=SO_SWITCH_ALL;
    //swFloor->whichChild=SO_SWITCH_NONE;

	Eigen::Vector3f up;
	up  << 0.0f,0.0f,1.0f;
	Eigen::Vector3f pos;
	pos << 0,0,0;
	SoSeparator *f = CoinVisualizationFactory::CreatePlaneVisualization(pos,up, 10000.0f, 0);
    floorSep->addChild(f);

	setupUI();

	this->dataFile = dataFile;
    this->markerMotionFile = markerMotionFile;
	load();

	viewer->viewAll();
    jumpToFrame(0);

	_pSensorMgr = SoDB::getSensorManager();
	_pSensorTimer = new SoTimerSensor(timerCB, this);
	_pSensorTimer->setInterval(SbTime(TIMER_MS/1000.0f));
	timerSensorAdded = false;

	//_pSensorTimer->unschedule();

}



MMMLegacyMotionViewerWindow::~MMMLegacyMotionViewerWindow()
{
	sceneSep->unref();
}


void MMMLegacyMotionViewerWindow::timerCB(void * data, SoSensor * sensor)
{
	MMMLegacyMotionViewerWindow *pWindow = static_cast<MMMLegacyMotionViewerWindow*>(data);
	pWindow->progress();
}


void MMMLegacyMotionViewerWindow::setupUI()
{
	UI.setupUi(this);
	viewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP);

	// setup
	viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
	viewer->setAccumulationBuffer(true);


	viewer->setGLRenderAction(new SoLineHighlightRenderAction);
	viewer->setTransparencyType(SoGLRenderAction::BLEND);
	viewer->setFeedbackVisibility(true);
	viewer->setSceneGraph(sceneSep);
	//viewer->setEventCallback(&MMMLegacyMotionViewerWindow::eventCallback, this);
	viewer->viewAll();

	connect(UI.checkBoxVisuRobot, SIGNAL(clicked()), this, SLOT(updateVisu()));
	connect(UI.checkBoxVisuValues, SIGNAL(clicked()), this, SLOT(updateVisu()));
	connect(UI.checkBoxVisuRoot, SIGNAL(clicked()), this, SLOT(updateVisu()));
    connect(UI.checkBoxVisuFloor , SIGNAL(clicked()), this, SLOT(updateVisu()));
    connect(UI.checkBoxVisuAntiAliasing , SIGNAL(stateChanged(int)), this, SLOT(setAntiAliasing(int)));
    connect(UI.buttonPlaySave , SIGNAL(clicked()), this, SLOT(saveMotion()));
	//connect(UI.comboBoxJoint, SIGNAL(activated(int)), this, SLOT(selectJoint(int)));
	connect(UI.horizontalSliderFrame, SIGNAL(valueChanged(int)), this, SLOT(sliderMoved(int)));
    connect(UI.cbFPS, SIGNAL(valueChanged(double)), this, SLOT(fpsChanged(double)));

    buildMarkerVisu();
}

void MMMLegacyMotionViewerWindow::updateVisu()
{
	bool showRobot = UI.checkBoxVisuRobot->isChecked();
	_bShowValues = UI.checkBoxVisuValues->isChecked();
    bool showRoot = UI.checkBoxVisuRoot->isChecked();
    bool showFloor = UI.checkBoxVisuFloor->isChecked();


    if (showFloor) swFloor->whichChild=SO_SWITCH_ALL; else
        swFloor->whichChild=SO_SWITCH_NONE;

    for(MMM::LegacyMotionList::iterator it = motions.begin(); it != motions.end(); it++)
    {

        MMM::LegacyMotionPtr motion = *it;
        VirtualRobot::RobotPtr robot = robots[motion->getName()];
        std::map<std::string, SoSeparator*>::iterator itSep = robotSeps.find(motion->getName());
        if(itSep == robotSeps.end())
        {
            itSep = robotSeps.insert(std::make_pair(motion->getName(), new SoSeparator())).first;
            sceneSep->addChild(itSep->second);
        }

        SoSeparator* robotSep = itSep->second;

        if (showRobot && robot && robotSep->getNumChildren()==0)
        {
            bool useColModel = false;
            SceneObject::VisualizationType colModelType = (useColModel)?SceneObject::Collision:SceneObject::Full;

            boost::shared_ptr<VirtualRobot::CoinVisualization> visualization = robot->getVisualization<CoinVisualization>(colModelType);
            SoNode* visualisationNode = NULL;
            if (visualization)
                visualisationNode = visualization->getCoinVisualization();

            if (visualisationNode)
                robotSep->addChild(visualisationNode);
        }
        if (!showRobot && robotSep->getNumChildren()>0)
        {
            robotSep->removeAllChildren();
        }
    }

	/*if (showValues && trajectory->getNrOfPoints()>0 && motionSep->getNumChildren()==0)
	{
		createMotionVisu(trajectory,motionSep);
	}
	if (!showValues && motionSep->getNumChildren()>0)
	{
		motionSep->removeAllChildren();
	}*/

	if (showRoot && rootCoordSep->getNumChildren()==0)
	{
		std::string rootTXT("Root Coordinate System");
		rootCoordSep->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(1.0f,&rootTXT));
	}
	if (!showRoot && rootCoordSep->getNumChildren()>0)
	{
		rootCoordSep->removeAllChildren();
	}

	QString df(dataFile.c_str());
	df = QString("Data File: ") + df;
	UI.labelDataFile->setText(df);
	UI.labelDataFile->setToolTip(df);

	updateTable();

    redraw();
}

void MMMLegacyMotionViewerWindow::buildMarkerVisu()
{
    markerSep->removeAllChildren();

    if (!markerMotion)
        return;

    MMM::LegacyMarkerDataPtr markerData = markerMotion->getFrame(0);
    if (!markerData)
        return;
    std::map<std::string, Eigen::Vector3f> data = markerData->data;
    SoSphere* sphere = new SoSphere;
    sphere->radius = 10;


    for (std::map<std::string, Eigen::Vector3f>::iterator it = data.begin(); it != data.end(); it++) {
        //create a sphere for each marker
        SoSeparator* sphereSep = new SoSeparator;
        Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
        globalPose.block(0, 3, 3, 1) = it->second;
        SoMaterial* marked = new SoMaterial;
        marked->ambientColor.setValue(0.2f, 0.2f, 1.0f);
        marked->diffuseColor.setValue(0.2f, 0.2f, 1.0f);
        marked->specularColor.setValue(0.5f, 0.5f, 0.5f);
        sphereSep->addChild(marked);

        sphereSep->addChild(CoinVisualizationFactory::getMatrixTransform(globalPose));
        sphereSep->addChild(sphere);

        //create marker labels
        SoSeparator* labelSep = new SoSeparator;
        Eigen::Vector3f labelVector;
        labelVector << 0, 30, 0;
        Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
        localPose.block(0, 3, 3, 1) += labelVector;
        localPose.block(0, 0, 3, 3) *= 2;
        //labelSep->addChild(marked);

        labelSep->addChild(CoinVisualizationFactory::getMatrixTransform(localPose));
        std::string txt = "marker";
        if (!it->first.empty())
            txt = it->first;

        labelSep->addChild(CoinVisualizationFactory::CreateBillboardText(txt));

        sphereSep->addChild(labelSep);
        markerSep->addChild(sphereSep);

    }
}

void MMMLegacyMotionViewerWindow::updateMarkerVisu(int frame)
{

    if (markerSep->getNumChildren() == 0)
        buildMarkerVisu();

    if (!markerMotion)
        return;
    MMM::LegacyMarkerDataPtr markerData = markerMotion->getFrame(frame);
    if (!markerData)
        return;
    std::map<std::string, Eigen::Vector3f> data = markerData->data;
    if ((int)markerSep->getNumChildren() < (int)data.size())
    {
        cout << "internal error..." << endl;
        return;
    }
//    std::string markerStr;// = markerMotion->getMarkerLabel(selectedMarker);

    size_t pos = 0;
    for (std::map<std::string, Eigen::Vector3f>::iterator it = data.begin(); it != data.end(); it++)
    {
        SoSeparator* s = (SoSeparator*)markerSep->getChild(pos);
        if (s->getNumChildren() < 3)
        {
            cout << "int err" << endl;
            return;
        }

//        // set material
//        SoMaterial* mat = (SoMaterial*)s->getChild(0);
//        if (it->first == markerStr )
//        {
//            mat->ambientColor.setValue(colorViconMarkerSelected.r, colorViconMarkerSelected.g, colorViconMarkerSelected.b);
//            mat->diffuseColor.setValue(colorViconMarkerSelected.r, colorViconMarkerSelected.g, colorViconMarkerSelected.b);
//            mat->specularColor.setValue(0.5f, 0.5f, 0.5f);


//            //mat->ambientColor.setValue(1.0f, 0.0f, 0.0f);
//            //mat->diffuseColor.setValue(1.0f, 0.0f, 0.0f);
//            //mat->specularColor.setValue(0.5f, 0.5f, 0.5f);
//            //globalPose.block(0, 0, 3, 3) *= 1.5; //make the sphere a bit more noticable
//        }
//        else
//        {
//            mat->ambientColor.setValue(colorViconMarker.r, colorViconMarker.g, colorViconMarker.b);
//            mat->diffuseColor.setValue(colorViconMarker.r, colorViconMarker.g, colorViconMarker.b);
//            mat->specularColor.setValue(0.5f, 0.5f, 0.5f);
//        }

        // set pose
        Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
        globalPose.block(0, 3, 3, 1) = it->second;
        SoMatrixTransform* mt = (SoMatrixTransform*)s->getChild(1);
        SbMatrix m_(reinterpret_cast<SbMat*>(globalPose.data()));
        mt->matrix.setValue(m_);
        pos++;

    }
}

void MMMLegacyMotionViewerWindow::fpsChanged(double newValue)
{
    if(newValue < 0.1)
        return;
    double interval = 1./newValue;
    _pSensorTimer->setInterval(SbTime(interval));
}

void MMMLegacyMotionViewerWindow::setAntiAliasing(int checkBoxState)
{
    viewer->setAntialiasing(checkBoxState, checkBoxState ? 8 : 1);
}

//void MMMLegacyMotionViewerWindow::createMotionVisu(VirtualRobot::TrajectoryPtr tr, SoSeparator* addToThisSep)
//{

//    if (!tr || !addToThisSep || !robotNodeSets)
//		return;
//	SoSeparator *res = new SoSeparator;
//	addToThisSep->addChild(res);
//	std::vector<SoSeparator*> motionSep;

//	float lineWidth = 2.0f;

//	// save current pose
//	Eigen::VectorXf startPose;
//    robotNodeSets->getJointValues(startPose);

//	// set pose to start
//	Eigen::VectorXf tmpPose;
//	tr->interpolate(0.0f,tmpPose);
//    robotNodeSets->setJointValues(tmpPose);

//	SoSphere *sphere = new SoSphere;
//	for (unsigned int i=0;i<tr->getDimension();i++)
//	{
//		SoSeparator* s=new SoSeparator;
//		res->addChild(s);
//		motionSep.push_back(s);

//        Eigen::Matrix4f globalPose = robotNodeSets->getNode(i)->getGlobalPose();
//		SoSeparator* start=new SoSeparator;
//		s->addChild(start);
//		start->addChild(CoinVisualizationFactory::getMatrixTransform(globalPose));
//		start->addChild(sphere);
//		std::stringstream ss;
//		ss << "Start:";
//        ss << robotNodeSets->getNode(i)->getName();
//		std::string startTXT = ss.str();
//		start->addChild(CoinVisualizationFactory::CreateBillboardText(startTXT));
//	}

//	// create lines
//	std::vector<Eigen::Matrix4f> startPoses(tr->getDimension());
//	int stepSize = 1;
//	if (tr->getNrOfPoints()>100)
//		stepSize = tr->getNrOfPoints() / 100;
//	if (stepSize<=0)
//		stepSize = 1;
//	for (unsigned int j=0;j<tr->getNrOfPoints()-stepSize;j+=stepSize)
//	{
//		Eigen::VectorXf currentPose = tr->getPoint(j);
//        robotNodeSets->setJointValues(currentPose);
//		for (unsigned int i=0;i<tr->getDimension();i++)
//		{
//            startPoses[i] = robotNodeSets->getNode(i)->getGlobalPose();
//		}
//		int nextPose = j+stepSize;

//		// ensure that the last point is visualized!
//		if (nextPose >= (int)tr->getNrOfPoints()-stepSize)
//			nextPose = tr->getNrOfPoints()-1;

//		currentPose = tr->getPoint(nextPose);
//        robotNodeSets->setJointValues(currentPose);
//		for (unsigned int i=0;i<tr->getDimension();i++)
//		{
//            motionSep[i]->addChild(CoinVisualizationFactory::createCoinLine(startPoses[i],robotNodeSets->getNode(i)->getGlobalPose(),lineWidth,0.5f,0.5f,0.5f));
//		}
//	}


//	// end visu
//	tr->interpolate(1.0f,tmpPose);
//    robotNodeSets->setJointValues(tmpPose);
//	for (unsigned int i=0;i<tr->getDimension();i++)
//	{
//        Eigen::Matrix4f globalPose = robotNodeSets->getNode(i)->getGlobalPose();
//		SoSeparator* end=new SoSeparator;
//		motionSep[i]->addChild(end);
//		end->addChild(CoinVisualizationFactory::getMatrixTransform(globalPose));
//		end->addChild(sphere);
//		std::stringstream ss;
//		ss << "End:";
//        ss << robotNodeSets->getNode(i)->getName();
//		std::string endTXT = ss.str();
//		end->addChild(CoinVisualizationFactory::CreateBillboardText(endTXT));
//	}

//	// restore original pose
//    robotNodeSets->setJointValues(startPose);

//}

void MMMLegacyMotionViewerWindow::progress() {
	_nLastPosition++;
	if (_nLastPosition>=UI.horizontalSliderFrame->maximum())
		_nLastPosition = UI.horizontalSliderFrame->maximum()-1;
	UI.horizontalSliderFrame->setSliderPosition(_nLastPosition);
	sliderMoved(_nLastPosition);
}

void MMMLegacyMotionViewerWindow::jumpToFrame(int pos)
{
    for(MMM::LegacyMotionList::iterator it = motions.begin(); it != motions.end(); it++)
    {

        MMM::LegacyMotionPtr motion = *it;
        VirtualRobot::RobotPtr robot = robots[motion->getName()];
        RobotNodeSetPtr rns = robotNodeSets[motion->getName()];
        if (!motion || pos<0 || pos >= (int)motion->getNumFrames() || !robot)
            continue;
        UI.lblCurFrame->setText(( QString::number(pos) + "/" + QString::number((int)motion->getNumFrames())));
        // update model/robot

        MMM::MotionFramePtr md = motion->getMotionFrame(pos);
        if (!md)
        {
            cout << "internal error" << endl;
            continue;
        }
        robot->setGlobalPose(md->getRootPose());
        if (rns)
        {
            if (md->joint.rows() != rns->getSize())
            {
                MMM_ERROR << "converted joint data does not fit size of model's kinematic chain:" << md->joint.rows() << "!=" << rns->getSize() << endl;
                continue;
            } else
            {
                MMM_INFO << "   Number of joint set: " << md->joint.rows() << endl;
            }
            MMM_INFO << md->joint << endl;
            rns->setJointValues(md->joint);
            std::vector<VirtualRobot::RobotNodePtr> rn = rns->getAllRobotNodes();
            for (size_t i = 0; i<rn.size(); i++){
                if (!checkStateInTable(i)) robot->setJointValue(rn[i], 0);
            }
        }
    }
    updateMarkerVisu(pos);
    updateTable();
}

void MMMLegacyMotionViewerWindow::sliderMoved(int pos)
{
    if (motions.size() == 0 || !(*motions.begin()))
		return;
	//cout << "Slider moved to pos:" << pos << endl;
	_nLastPosition = pos;
	float t = float(pos) / float(UI.horizontalSliderFrame->maximum());
    int motionPos = (int)(t * motions.at(0)->getNumFrames());
	jumpToFrame(motionPos);
}

void MMMLegacyMotionViewerWindow::closeEvent(QCloseEvent *event)
{
	quit();
	QMainWindow::closeEvent(event);
}


int MMMLegacyMotionViewerWindow::main()
{
	SoQt::show(this);
	SoQt::mainLoop();
	return 0;
}


void MMMLegacyMotionViewerWindow::quit()
{
	std::cout << "MMMLegacyMotionViewerWindow: Closing" << std::endl;
	this->close();
	SoQt::exitMainLoop();
}

void MMMLegacyMotionViewerWindow::redraw()
{
	//viewer->scheduleRedraw();
	//this->update();
	//UI.frameViewer->update();
	viewer->scheduleRedraw();
	//this->activateWindow();
}



void MMMLegacyMotionViewerWindow::load()
{
	cout << "Loading MMM data..." << endl;

	// remove robot visu
	for (std::map<std::string, SoSeparator*>::iterator it = robotSeps.begin(); it != robotSeps.end(); it++)
	{
		if (it->second) it->second->removeAllChildren();
	}

    MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
    std::vector < std::string > motionNames = r->getMotionNames(dataFile);

    if (motionNames.size() == 0)
	{
		MMM_ERROR << "no motions in file " << endl;
		return;
	}

    int frameCount = -1;
    for(size_t i = 0; i < motionNames.size(); i++)
    {
        MMM::LegacyMotionPtr motion = r->loadMotion(dataFile,motionNames[i]);
        if(!motion)
            continue;
        cout << "Loading motion: " << motion->getName() << endl;
        if(frameCount != -1 && frameCount != (int) motion->getNumFrames())
        {
            MMM_ERROR << "Error whole loading motions: the frame count between two motions do not match. All motions need to have the same framecount." << endl;
            return;
        }
        else
            frameCount = motion->getNumFrames();

        cout  << "model file: " << motion->getModel()->getFilename() << endl;

        if ( motion->getModel())
        {
            robots[motion->getName()] = MMM::SimoxTools::buildModel( motion->getModel());
            MMM::SimoxTools::updateInertialMatricesFromModels(robots[motion->getName()]);
        }
        VirtualRobot::RobotPtr robot = robots[motion->getName()];

        if (motion)
        {
            std::vector<std::string> jointNames = motion->getJointNames();
            if (jointNames.size()>0)
            {
                std::string rnsName("MMMLegacyMotionViewerRNS");
                if (robot->hasRobotNodeSet(rnsName))
                    robot->deregisterRobotNodeSet(robot->getRobotNodeSet(rnsName));
                robotNodeSets[motion->getName()] = RobotNodeSet::createRobotNodeSet(robot, rnsName, jointNames, "", "", true);
            }
            UI.horizontalSliderFrame->setRange(0, motion->getNumFrames());
            setupTable();
        }
        motions.push_back(motion);
    }
	cout << "Loading MMM data... done..." << endl;

    if(!markerMotionFile.empty())
    {
        MMM::LegacyMotionReaderC3DPtr c(new MMM::LegacyMotionReaderC3D());
        markerMotion = c->loadC3D(markerMotionFile);
        if (markerMotion)
        {
            std::vector<std::string> markerLabels = markerMotion->getMarkerLabels();
            for (size_t i = 0; i < markerLabels.size(); i++)
                std::cout << markerLabels[i] << std::endl;
        }
    }

	updateVisu();

    sliderMoved(0);
}


void MMMLegacyMotionViewerWindow::setupTable()
{
	// put robot_node_names in QStringList
    if (robots.size() == 0 || robotNodeSets.size() == 0 || !robotNodeSets.begin()->second)
        return;
    std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSets.begin()->second->getAllRobotNodes();
	QStringList list_rn_names;
	for (size_t i=0;i<rn.size();i++) list_rn_names << rn[i]->getName().c_str();

	QStringList list_col;
	list_col<<"Joint"<<"Value";

	UI.tableWidget->setRowCount(list_rn_names.size());
	UI.tableWidget->setColumnCount(list_col.size());
	UI.tableWidget->setHorizontalHeaderLabels(list_col);

	// table_items for each robot_node
	foreach (QString str, list_rn_names) {
		QTableWidgetItem *item = new QTableWidgetItem(str);
		QTableWidgetItem *item2 = new QTableWidgetItem("N/A");
		item->setCheckState(Qt::Checked);
		UI.tableWidget->setItem(list_rn_names.indexOf(str),0 , item);
		UI.tableWidget->setItem(list_rn_names.indexOf(str),1 , item2);
	}
}

void MMMLegacyMotionViewerWindow::updateTable(){
	//dirty funktioniert nur weil feste reihenfolge in robot node set angenommen wird
	if (robots.size() == 0 || motions.size() == 0 || !motions.at(0) || robotNodeSets.size()==0 || !robotNodeSets.begin()->second)
		return;
	std::stringstream ss;
    ss << "Pose [" << _nLastPosition << "|" << motions.at(0)->getNumFrames() << "]";
	QString df = QString::fromStdString(ss.str());
	UI.labelDataInfo->setText(df);
	UI.labelDataInfo->setToolTip(df);
	if (!_bShowValues) return;
	//VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("Robot");
	std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSets.begin()->second->getAllRobotNodes();
	for (size_t i=0;i<rn.size();i++){
		if(UI.tableWidget->item((int)i, 1)!=NULL) 
            UI.tableWidget->item((int)i, 1)->setText(QString::number(rn[i]->getJointValue(),'f',6));

	}
}


// is checkbox in row, col= 0 checked?
bool MMMLegacyMotionViewerWindow::checkStateInTable(int row){
	if (UI.tableWidget->rowCount() == 0)
		return false;
	return UI.tableWidget->item((int)row,0)->checkState()==0 ? 0: 1;
}


void  MMMLegacyMotionViewerWindow::lockSelectedJointValuesFromTable(const Eigen::VectorXf poseVec)
{
    if (robots.size() == 0 || robotNodeSets.size() == 0 || !robotNodeSets.begin()->second) return;

    robotNodeSets.begin()->second->setJointValues(poseVec);

    std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSets.begin()->second->getAllRobotNodes();
	for (size_t i=0;i<rn.size();i++){
        if (!checkStateInTable(i)) robots.begin()->second->setJointValue(rn[i],0);
	}
}

void MMMLegacyMotionViewerWindow::on_tableWidget_cellClicked(int row, int column)
{
	//TODO implement altering cell values here
	if (column==1)
		cout << "on_tableWidget_cellClicked int row, int column  " << row << "  " <<column << endl;
}


void MMMLegacyMotionViewerWindow::on_playButton_clicked()
{
	if (!timerSensorAdded)
		_pSensorMgr->insertTimerSensor(_pSensorTimer);
	timerSensorAdded = true;

	//_pSensorTimer->schedule();
}

void MMMLegacyMotionViewerWindow::on_stopButton_clicked()
{
	if (timerSensorAdded)
		_pSensorMgr->removeTimerSensor(_pSensorTimer);
	timerSensorAdded = false;

}

void MMMLegacyMotionViewerWindow::saveMotion()
{
    if (motions.size() == 0 || !(*motions.begin()))
        return;
    //cout << "Slider moved to pos:" << pos << endl;
    for (int i = 0; i < (int)motions.at(0)->getNumFrames(); i++)
    {
        jumpToFrame(i);
        saveScreenshot();
    }
}

void MMMLegacyMotionViewerWindow::saveScreenshot()
{
    static int counter = 0;
    SbString framefile;

    framefile.sprintf("sequence/MMMLegacyMotionViewerWindow_Frame%06d.png", counter);
    counter++;

    viewer->getSceneManager()->render();
    viewer->getSceneManager()->scheduleRedraw();
    QGLWidget* w = (QGLWidget*)viewer->getGLWidget();

    QImage i = w->grabFrameBuffer();
    bool bRes = i.save(framefile.getString(), "PNG");
    if (bRes)
    cout << "wrote image " << counter << endl;
    else
    cout << "failed writing image " << counter << endl;
}

